%**************************************************************************
%               Juan Antol�n-D�az 2012
%**************************************************************************

% Kalman Smoother recursion

function [xtt_s, Sig_s] = KalmanSmoother(xtt,Sigtt,Sigttminus1,F,D,a)

k = size(F,1);
T = size(xtt,1);

if isempty(D), D = zeros(k,1); end
if isempty(a), a = zeros(T,1); end

xtt = xtt';

% allocating memory

Sig_s   = zeros(k,k,T);       % E[xhat_t,xhat_t']
xtt_s   = zeros(k,T);         % Ehat_t[x_{t+1}] 

% Final values
Sig_s(:,:,T) = Sigtt(:,:,T);        
xtt_s(:,T)   = xtt(:,T);         

for h=1:(T-1)
    
    at = a(T-h,:)';
    
    if ~ismatrix(F)
        Ft = F(:,:,T-h);
    else Ft = F; 
    end    
               
    J = (Sigtt(:,:,T-h)*Ft')/(Sigttminus1(:,:,T-h+1));  % Kalman gain
    
    xtt_s(:,T-h) = xtt(:,T-h) + J * (xtt_s(:,T-h+1) - Ft*xtt(:,T-h) - D*at);           % update for x
    Sig_s(:,:,T-h) = Sigtt(:,:,T-h) + J * (Sig_s(:,:,T-h+1)-Sigttminus1(:,:,T-h+1)) * J';         % update for Sig
       
end

xtt_s = xtt_s';


